import random

def clip(val, min_val, max_val):
    if val < min_val:
        return min_val
    if val > max_val:
        return max_val
    return val

class Quadcopter:
    """四轴飞行器模型
    """
    def __init__(self, initial_height=0, thrust_noise = 3.0):
        # 重力加速度 m/s^2
        self.__gravity = 9.81
        self.__max_thrust = 10 * 9.81
        self.__thrust_noise = thrust_noise
        
        self.__height = initial_height
        self.__speed = 0

    def fly(self, thrust, delta_t=0.01):
        
        # 添加一些随机的高斯噪声
        thrust = random.gauss(thrust, self.__thrust_noise)
        
        # clip thrust
        # thrust = clip(thrust, 0, self.max_thrust)
        
        # F = ma = mg = m ( a - g )
        acc = thrust - self.__gravity
        # 速度 = 加速度 * delta_t
        self.__speed += acc * delta_t
        # 位移 = 速度 * delta_t
        self.__height += self.__speed * delta_t
        
        # clip height
        self.__height = clip(self.__height, 0, 14)

    def get_noise(self):
        return self.__thrust_noise
    
    def set_noise(self, noise):
        self.__thrust_noise = noise

    def get_height(self):
        return self.__height

if __name__ == '__main__':
        
    # 示例用法
    quadcopter = Quadcopter()
    for i in range(10):
        quadcopter.fly(10)  # 模拟升力为10
        print("当前高度：", quadcopter.get_height())

    print("------------------------------")
    # 设置新高度
    quadcopter.__height = 20
    for i in range(10):
        quadcopter.fly(1.0)  # 模拟升力为9.0
        print("当前高度：", quadcopter.get_height())
        
    print("更新后的高度：", quadcopter.get_height())
